
#ifndef __MLSUTILS_H_
#define __MLSUTILS_H_

#include <iostream>
#include <cmath>

/// Constant values used for computing cubic roots
static const Number _1DIV3 = 0.333333; // 1/3
static const Number _1DIV9 = 0.111111; // 1/9
static const Number _1DIV54 = 0.0185185; // 1/54
static const Number SQRT3= 1.73205080; // sqrt(3.0);
static const Number EEPSILON = 1e-5;

inline Number sgn(Number v) {
   if (v<0) return -1;
   else return 1;
}

Number getCubitRoot_A(const Number a, const Number b, const Number c) {
   
   Number Q = (3.0*b - a*a)*_1DIV9;
   Number R = (9.0*a*b - 27.0*c - 2.0*a*a*a)*_1DIV54;
   Number D = Q*Q*Q + R*R;
   
   if (D>=0) {  /// one real, two complex :: D==0 root reals, at least two are equal 
      Number sqrtD = sqrt(D);
      Number S = sgn(R+sqrtD)*pow(std::abs(R+sqrtD), _1DIV3);
      Number T = sgn(R-sqrtD)*pow(std::abs(R-sqrtD), _1DIV3);
      return -a*_1DIV3 + S + T;
   }
   else {   /// all root are real and unequal
      Number theta = acos(R/sqrt(-Q*Q*Q));
      return 2.0*sqrt(-Q)*cos(theta*_1DIV3)-a*_1DIV3;
   }
}

Number getCubitRoot_B(const Number a, const Number b, const Number c) {
   
   Number p = b - (a*a)/3.0;
   Number q = c + (2.0*a*a*a - 9.0*a*b)/27.0;
   
   Number modulo = sqrt(-(p*p*p)/27.0);
   Number temp = -(q*(1.0-EEPSILON))/(2.0*modulo);
   
   return 2.0*(sqrt(-p/3.0))*cos(acos(temp)/3.0) - a/3.0;
}

/// cardano's original formula. 
Number getCubitRoot_C(const Number a, const Number b, const Number c) {
   
   Number p = b - (a*a)/3.0;
   Number q = c + (2.0*a*a*a-9.0*a*b)/27.0;
   
   Number k = sqrt(4.0*p*p*p + 27.0*q*q);
   Number t =(-3.0*q*SQRT3 + k)/(6.0*SQRT3);
   Number u =( 3.0*q*SQRT3 + k)/(6.0*SQRT3);
   
   Number y = pow(t,_1DIV3) - pow(u,_1DIV3);
   return y-a/3.0;
}

Number computeDistanceMinSegment(const Point3 &v, const Segment3 &segment, Number &t) {
   
   const Point3 &a = segment.source();
   const Point3 &b = segment.target();
   
   const Vector3 &ba = b-a;
   const Vector3 &va = v-a;
   const Vector3 &vb = v-b;
   
   const Number &norma2aibi = ba*ba;
   const Number &dotvaibiai = va*ba;
   
   Number dminvaibi;
   if (dotvaibiai<=0)                  { dminvaibi = sqrt(va*va); t=1.0;}
   else if (dotvaibiai>=norma2aibi)    { dminvaibi = sqrt(vb*vb); t=0.0;}
   else                                { 
      dminvaibi = sqrt(va*va - ((dotvaibiai*dotvaibiai)/norma2aibi)); 
      t = 1.0-dotvaibiai/norma2aibi;
   }
   
   return dminvaibi;
}

#endif
